/* 
 * File:   CGpioLed.cpp
 * Author: viking
 * 
 * Created on February 2, 2014, 3:21 PM
 */

#include <stdio.h>
#include <string.h>
#include <string>
#include <signal.h>
#include <unistd.h>
#include <time.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "CConnection.h"
#include "CBallCircle.h"
#include "CTargetInfo.h"
#include "CVideoFrame.h"
#include "CVideoFrameQueue.h"
#include "CConnectionServer.h"
#include "CGpioLed.h"
#include "CBallDetector.h"
#include "CFrameGrinder.h"

CGpioLed::CGpioLed()
{
    //Don't adjust the state of the LED more often than this interval
    m_updateLedIntervalSeconds = 0.5;
}

CGpioLed::CGpioLed(const CGpioLed& orig)
{
    local_setVisionOkLED(false);
    m_enableStateCurrent = false;
    CFrameGrinder::getTicks(&m_timeStamp);
    m_initOK = false;
    if (!local_setVisionOkLED(false))
    {
        printf("GPIO LED init failed\n");
    }
}

CGpioLed::~CGpioLed()
{
}

void CGpioLed::setVisionOkLED(bool isVisionOriented, struct timespec timeStamp)
{
    if (isVisionOriented != m_enableStateCurrent)
    {
        double elapsedSecs = CFrameGrinder::getDeltaTimeSeconds(m_timeStamp, timeStamp);
        if (elapsedSecs > m_updateLedIntervalSeconds)
        {
            if (local_setVisionOkLED(isVisionOriented))
            {
                m_enableStateCurrent = isVisionOriented;
            }
            m_timeStamp = timeStamp; // Even if the attempt fails, don't try to do this more often than the specified interval
        }
    }
}

bool CGpioLed::local_initVisionOkLED()
{
    try
    {
        FILE* hLED = NULL;
        char setValue[4];

        m_initOK = false;

        // Export the GPIO pin    -- BeagleBone Black P9 pin 12 --> GPIO #60
        // Details:   BeagleBone Black pin 12 is gpio_1[28]
        //            There are 4 controllers for 32 GPIOs each, so (1*32)+28 = 60
        if ((hLED = fopen("/sys/class/gpio/export", "ab")) == NULL)
        {
            // This is normal when running on a non-BBB laptop - just ignore and continue
            return false;
        }
        strcpy(setValue, "60");
        fwrite(&setValue, sizeof (char), strlen(setValue), hLED);
        fflush(hLED);
        fclose(hLED);

        // Set the direction of the pin to "out"
        if ((hLED = fopen("/sys/class/gpio/gpio60/direction", "rb+")) == NULL)
        {
            printf("Problem opening handle to export GPIO pin 60\n");
            return false;
        }
        strcpy(setValue, "high");
        fwrite(&setValue, sizeof (char), strlen(setValue), hLED);
        fflush(hLED);
        fclose(hLED);

        m_initOK = true;
        return true;
    }
    catch (...)
    {
    }
    printf("local_initVisionOkLED() terminated unexpectedly\n");
    return false;
}

bool CGpioLed::local_setVisionOkLED(bool isVisionOriented)
{
    if (!m_initOK)
    {
        if (!local_initVisionOkLED())
        {
            return false;
        }
    }
    try
    {
        FILE* hLED = NULL;
        char setValue[4];

        // Turn the LED on or off depending on the input
        if ((hLED = fopen("/sys/class/gpio/gpio60/value", "rb+")) == NULL)
        {
            printf("Problem opening handle to export GPIO pin 60\n");
            return false;
        }
        if (isVisionOriented)
        {
            strcpy(setValue, "1");
        }
        else
        {
            strcpy(setValue, "0");
        }
        fwrite(&setValue, sizeof (char), strlen(setValue), hLED);
        fflush(hLED);
        fclose(hLED);

        return true;
    }
    catch (...)
    {
    }
    printf("local_setVisionOkLED() terminated unexpectedly\n");
    return false;
}

